﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using PloobsEngine.Utils;
using PloobsEngine.Physics.Bepu;
using PloobsEngine.Draw;

namespace Etapa1.SteeringExperiments
{
    public enum BehaviorType
    {
        avoidance = 0x00000,
        seek = 0x00002,
        flee = 0x00004,
        arrive = 0x00008,
        wander = 0x00010,
        cohesion = 0x00020,
        separation = 0x00040,
        allignment = 0x00080,
        obstacle_avoidance = 0x00100,
        wall_avoidance = 0x00200,
        follow_path = 0x00400,
        pursuit = 0x00800,
        evade = 0x01000,
        interpose = 0x02000,
        hide = 0x04000,
        flock = 0x08000,
        offset_pursuit = 0x10000,
    };
    
        
    public class SteeringBehavior
    {
        private struct behaviorStruct
        {
            public behaviorStruct( Steering Steer, float weight,int Priority)
            {
                this.Steer = Steer;
                this.Priority = Priority;
                this.Weight = weight;
            }

            public Steering Steer;
            public int Priority;
            public float Weight ;
        }

        private class priorytyComparer : IComparer<behaviorStruct>
        {

            #region IComparer<behaviorStruct> Members

            public int Compare(behaviorStruct x, behaviorStruct y)
            {
                if (x.Priority > y.Priority)
                    return 1;
                else if (y.Priority > x.Priority)
                    return -1;
                else
                    return 0;
            }

            #endregion
        }

        List<behaviorStruct> _steers = new List<behaviorStruct>();
        priorytyComparer pc = new priorytyComparer();

        public bool HasBehavior(BehaviorType type)
        {
            return _steers.Count((a) => a.Steer.Type == type) != 0;
        }

        public void AddBehavior(Steering steer,int priority,float weight)
        {
            behaviorStruct b = new behaviorStruct(steer,weight, priority);
            _steers.Add(b);
            _steers.Sort(pc);
        }

        public void AddBehavior(Steering steer)
        {
            behaviorStruct b = new behaviorStruct(steer,1,10);            
            _steers.Add(b);
            _steers.Sort(pc);
        }

        public BehaviorTp GetBehavior<BehaviorTp>(BehaviorType type) where BehaviorTp : Steering
        {
            int num = _steers.Count((x) => x.Steer.Type == type);
            if (num == 0)
                return default(BehaviorTp);

            return (BehaviorTp)(_steers.First<behaviorStruct>((a) => a.Steer.Type == type)).Steer;
        }

        public void RemoveBehavior(Steering steer)
        {
            behaviorStruct bs = _steers.First<behaviorStruct>((a) => a.Steer == steer);
            _steers.Remove(bs);            
        }

        Vector3 acum = Vector3.Zero;
        public void Update(GameTime gameTime,SteeringEntityInfo infoObj)
        {
            Vector3 _steeringForce = Vector3.Zero;

            for (int i = 0; i < _steers.Count(); i++)
            {
                if (_steers[i].Steer.SteeringState != SteeringState.Innactive)
                {
                    if (TruncateVector(ref _steeringForce, _steers[i].Steer.CalculateSteering(infoObj) * _steers[i].Weight, infoObj.MaxForce) == false)
                        break;                    
                }
            }            

            ///test yet
            ObjectMover pi = infoObj.Obj.PhysicObject as ObjectMover;
            _steeringForce.Y = 0;
            //MathUtilities.BlendIntoAccumulator(0.3f, _steeringForce, ref acum);
            //_steeringForce = acum;
            //_steeringForce.Y = 0;

            Drawing.Draw3dLine(infoObj.Obj.Position, infoObj.Obj.Position + _steeringForce * 15, Color.Aqua);
            float intensity = _steeringForce.Length();
            
            if (intensity > 0f)
            {
                pi.GoalVelocity += _steeringForce / infoObj.ForceToVelocityCompensation;

                if (pi.BepuEntityObject.Entity.InternalLinearVelocity.Length() > 0.5f)
                {
                    float angle = (float)VectorUtils.FindAngleBetweenTwoVectors2D(pi.BepuEntityObject.Entity.InternalLinearVelocity, pi.BepuEntityObject.Entity.InternalOrientationMatrix.Forward);
                    if (angle > 0.001 || angle < -0.001)
                    {
                        float a = MathHelper.ToDegrees(-angle);
                        Matrix m  = pi.BepuEntityObject.Entity.InternalOrientationMatrix * Matrix.CreateRotationY(-angle);
                        pi.GoalOrientation = Quaternion.CreateFromRotationMatrix(m);
                    }
                }                
            }
          
        }

        private bool TruncateVector(ref Vector3 agregated ,Vector3 vector, float max)
        {
            agregated.Y = 0;
            vector.Y = 0;
            agregated += vector;

            if (agregated.Length() > max)
            {
                agregated.Normalize();
                agregated *= max;            
                return false;
            }
            else
            {                
                return true;
            }
            
        }

    }
}


/*

Vector3 acum = Vector3.Zero;
        public void Update(GameTime gameTime,SteeringEntityInfo infoObj)
        {
            Vector3 _steeringForce = Vector3.Zero;

            for (int i = 0; i < _steers.Count(); i++)
            {
                if (_steers[i].Steer.SteeringState != SteeringState.Innactive)
                {
                    if (TruncateVector(ref _steeringForce, _steers[i].Steer.CalculateSteering(infoObj) * _steers[i].Weight, infoObj.MaxForce) == false)
                        break;                    
                }
            }            

            ///test yet
            BepuEntityObject pi = infoObj.Obj.PhysicObject as BepuEntityObject;
            _steeringForce.Y = 0;
            MathUtilities.BlendIntoAccumulator(0.3f, _steeringForce, ref acum);
            _steeringForce = acum;
            _steeringForce.Y = 0;

            Drawing.DrawLine(infoObj.Obj.Position, infoObj.Obj.Position + _steeringForce * 15, Color.Aqua);
            float intensity = _steeringForce.Length();
            
            if (intensity > 0f)
            {
                pi.Entity.InternalLinearVelocity += _steeringForce/infoObj.ForceToVelocityCompensation ;

                if (pi.Entity.InternalLinearVelocity.Length() > 0.5f)
                {
                    float angle = (float)VectorUtils.FindAngleBetweenTwoVectors2D(pi.Entity.InternalLinearVelocity, pi.Entity.InternalOrientationMatrix.Forward);
                    if (angle > 0.001 || angle < -0.001)
                    {
                        float a = MathHelper.ToDegrees(-angle);
                        //Vector3 res = Vector3.Transform(infoObj.Obj.PhysicObject.FaceVector, Matix.CreateRotationY(-angle));
                        //res = VectorUtils.RotateAboutGlobalY(infoObj.Obj.PhysicObject.FaceVector, -angle);
                        pi.Entity.InternalOrientationMatrix = pi.Entity.InternalOrientationMatrix * Matrix.CreateRotationY(-angle);
                        //Vector3 VectorTransformed = (pi.Entity.InternalOrientationMatrix * Matrix.CreateRotationY(-angle)).Forward;
                        //Vector3 teste = pi.Entity.InternalOrientationMatrix.Forward;
                        //angle = (float)VectorUtils.FindAngleBetweenTwoVectors2D(velocidade, pi.Entity.InternalOrientationMatrix.Forward);
                        //a = MathHelper.ToDegrees(-angle);
                    }
                }                
            }
          
        }


*/